import lejos.nxt.*;
import lejos.nxt.comm.RConsole;
import lejos.util.TextMenu;
import lejos.robotics.navigation.*;


public class NewMenu implements ButtonListener {
	Pilot tPilot;
	SimpleNavigator sNavigator;	
	UltrasonicSensor uSensor;

	boolean isDebugOn;
	//The minimum distance to an object
	private static float OBJDIS = 25;

	public void NewMenu() {
		this.setDebug(false);
		Button.ESCAPE.addButtonListener(this);
	}

	public boolean ObjectInFront() {
		float distance = uSensor.getDistance();
		if( this.getDebug() ) {
			RConsole.println("Distance to object is : " + distance + " " +((distance > OBJDIS ) ? "No object" : "An Object"));
		}
		return (distance > OBJDIS ) ? false : true;		
	}

	public float getSonicRead() {
		float read = 0, tmp;

		for( int i = 0; i < 10; i++) {
			tmp =  uSensor.getDistance();
			read += tmp;
			read /= 10;	
			try {
				Thread.sleep(50);
			}catch(Exception e) {};
		}

		return read;
	}

	public int whereToGo() {
		/*
			Return value is :
				0, if there is nothing to the right or to the left.
				1, if there is nothing to the right.
				2, if there is nothing to the left.
				3, if there is nothing to either direction.			
		*/
		Motor.B.rotateTo(-90);
		while( Motor.B.isRotating() ) {}
		boolean left = this.ObjectInFront();
		Motor.B.rotateTo(0);
		while( Motor.B.isRotating() ) {}
		//Now we look to the right, to see if there is something that way
		Motor.B.rotateTo(90);
		while( Motor.B.isRotating() ) {}
		boolean right = this.ObjectInFront();
		Motor.B.rotateTo(0);
		while( Motor.B.isRotating() ) {}

		if( right == true && left != true ) {
			return 1;
		} else if( right != true && left == true ) {
			return 2;
		} else if( right == true && left == true ) {
			return 3;
		} else {
			return 0;
		}
	}

	public void searchDrive(float x, float y, boolean followWall ) {
		boolean detectWall = true;
		int detectCount = 0;
		int   Direction = 0;
		float BANE = 0;
	
		float currentX = this.sNavigator.getX(), 
		      currentY = this.sNavigator.getY();

		float maxX = 0;
		float maxY = 0;
		float minX = 0;
		float minY = 0;

		float stepX = 10,// ((x-currentX)/10),
		      stepY = 10; //((y-currentY)/10);		

		float tmpX = currentX,
		      tmpY = currentY;

		float distanceToWall = 0;


		float previousError = 0,
		      Integral      = 0,
		      Error	    = 0;
//		      Output	    = 0;

		float 	Kp = 0.95f,
			Ki = 0.7f,
			Kd = 0.125f;

		LCD.clearDisplay();
		LCD.refresh();
		System.out.println("Running search");

	/*	if( this.getDebug() ) {
			RConsole.println("Received ( " + x + " , " + y + " ) and the current ( " + currentX + " , " + currentY + " )");
			RConsole.println(" (x1 - x0)/10 = " + stepX  );
			RConsole.println("Kp : " + Kp + " Ki : " + Ki + " Kd : " + Kd );			
		}*/
		/*
    		 * if we want to follow wall, then it will take an first reading here.
     		 */
		if( followWall ) {
	
			Motor.B.rotateTo(-90);
			while( Motor.B.isRotating() ) {};
			BANE = this.getSonicRead();

			if( this.getDebug() ) {
				RConsole.println("Default distance is " + BANE);
			}
			
			try {
				Thread.sleep(100);
			} catch(Exception e) {};

			Motor.B.rotateTo(0);
			while( Motor.B.isRotating() ) {};
		}		

		while(true) {
/*
			if(this.sNavigator.isMoving() == false) {
				this.sNavigator.updatePosition();
				System.out.println("Updating");
				
			}
*/
			switch(Direction) {
				case 0:
					tmpX += stepX;

					if(detectWall == false){
						detectCount++;
					}
					if(detectCount >= 2) {
						detectWall = true;
						detectCount = 0;
					}
				break;

				case 1:
					tmpY -= stepY;

					if(detectWall == false){
						detectCount++;
					}
					if(detectCount >= 2) {
						detectWall = true;
						detectCount = 0;
					}
				break;

				case 2:
					tmpX -= stepX;

					if(detectWall == false){
						detectCount++;
					}
					if(detectCount >= 2) {
						detectWall = true;
						detectCount = 0;
					}

				break;

				case 3:
					tmpY += stepY;

					if(detectWall == false){
						detectCount++;
					}
					if(detectCount >= 2) {
						detectWall = true;
						detectCount = 0;
					}

				break;
				default:
					break;
			}

			if(this.getDebug() ) {
				RConsole.println("tmpX : " + tmpX + " tmpY : " + tmpY + " Direction : " + Direction + " detectWall : " + detectWall + " detectCount : " + detectCount);
			}
		
			if(this.ObjectInFront() && detectWall == true) {
				this.sNavigator.stop();
				detectWall = false;
				//

				
				if( this.getDebug() ) {
					RConsole.println("Hit an wall, going Im I going left, right or stop?");
				}

				int dis = this.whereToGo();
				switch(dis) {
					case 0:


						this.sNavigator.stop();
					break;
					case 1:
						System.out.println("Left");
	
						try {
							Thread.sleep(500);
						} catch(Exception e) {};
	
						if( this.getDebug() ) {
							RConsole.println("Going left");
						}
						Direction++;

						if(Direction > 3) {
							Direction = 0;
						}

						if( followWall ) {
							previousError = 0;
			      				Integral      = 0;
				   			Error	    = 0;
						}
					break;
					case 2:
					case 3:
						System.out.println("Right");
						try {
							Thread.sleep(500);
						} catch(Exception e) {};
	
						if( this.getDebug() ) {
							RConsole.println("Going right");
						}
						Direction++;
						if( followWall ) {
							previousError = 0;
			      				Integral      = 0;
				   			Error	    = 0;
						}
					break;
				}

				if( followWall == true ) {
					int oldDirection = Direction -1;
					switch(oldDirection) {
						case 0:
							maxX = tmpX;
							minY = tmpY;
							tmpY = 0;
							this.sNavigator.updatePosition();
							break;
						case 1:
							maxY = tmpY;
							tmpX = maxX;
							this.sNavigator.updatePosition();
							break;
						case 2:
							tmpY = maxY;
							minX = tmpX;
							tmpX = 0;
							this.sNavigator.updatePosition();
							break;
						case -1:
							if(tmpY < minY) {
								minY = tmpY;
							}

							if(tmpX < minX ) {
								minX = tmpX;
							}

							tmpX = 0;
							tmpY = 0;
							this.sNavigator.updatePosition();
						break;
					}
				 }

			}

			try {
				Thread.sleep(1500);
			} catch(Exception e) {};

			if( followWall) {
				Motor.B.rotateTo(-90);
				while( Motor.B.isRotating() ) {
				}
				distanceToWall = this.getSonicRead();


				try {
					Thread.sleep(100);
				}catch(Exception e) {};

				Motor.B.rotateTo(0);
				while( Motor.B.isRotating() ) {

				}
			
				Error 	  	 = BANE - distanceToWall;

				float Derivative = Error - previousError; 				
				//Our PID Regulator
				Integral   	+= Error;
				Derivative *= Kd;
				Integral *= Ki;

				switch(Direction) {
					case 0:
						tmpY = (Kp*Error) + (Integral) + (Derivative);				
					break;
					case 1:
						tmpX = (Kp*Error) + (Integral) + (Derivative);
					break;
					case 2:
						tmpY = (Kp*Error) + (Integral) + (Derivative);				
					break;
					case 3:
						tmpX = (Kp*Error) + (Integral) + (Derivative);
					break;
				}

				
				//Setting the previous error value			
				previousError = Error;



				if( this.getDebug() ) {
					RConsole.println("tmpY : " + tmpY +  " I : " + Integral + " D : " + Derivative + " Error : " + Error);
					RConsole.println("BANE : " + BANE + " distanceToWall : " + distanceToWall );
				}
		
			}
			this.sNavigator.goTo( tmpX, tmpY, true);
			// Sweep function

		}
	
	}

	// from ButtonListener
	public void buttonPressed(Button b) {
		LCD.clear();
		//Going to check if the button is ESCAPE
		if( this.getDebug() ) {
			RConsole.close();
		}
		System.exit(0);
	}

	public void buttonReleased(Button b) {
	}
	// end

	public void RunOutline() {
		this.searchDrive(30,30, false);
	}

	public void setDebug(boolean tmp ) {
		isDebugOn = tmp;
	}

	public boolean getDebug() {
		return isDebugOn;
	}

	public static void main(String[] args) {
		NewMenu Robot = new NewMenu();
		String[] MainMenuText = {"Center Arm", "Turn Debug ON", "Set Mode", "Run Program", "Quit Program"};
		int MainSelect = 0;
		int ModeSelect = 0;

//		Robot.tPilot = new TachoPilot(10.0f,20.0f, Motor.A, Motor.C);
		Robot.tPilot = new TachoPilot(9.0f, 15.7f, Motor.A, Motor.C);
		Robot.sNavigator = new SimpleNavigator(Robot.tPilot);
		Robot.uSensor = new UltrasonicSensor(SensorPort.S2);

		Robot.tPilot.setMoveSpeed(10);
		Robot.tPilot.setTurnSpeed(50);//20);
		

		for(;;) {
			TextMenu MainMenu = new TextMenu(MainMenuText, 0, "Menu");

			MainSelect = MainMenu.select(MainSelect);
	
			if(MainSelect < 0) return;

			switch(MainSelect) {
				case 0: //Center arm
					int angle = 0;

					if( Robot.getDebug() ) {
						RConsole.println("" + MainMenuText[MainSelect]);
					}

					LCD.clearDisplay();
					LCD.refresh();

					System.out.println("Calibrate rotor");

					do {
						if( Button.LEFT.isPressed() ) {
							angle = -5;
							Motor.B.rotate(angle);

							if( Robot.getDebug() ) {
								RConsole.println("LEFT : " + Motor.B.getStopAngle());
							}

						}

						if( Button.RIGHT.isPressed() ) {
							angle = 5;
							Motor.B.rotate(angle);

							if( Robot.getDebug() ) {
								RConsole.println("RIGHT : " + Motor.B.getStopAngle());
							}
						}

						try {
							Thread.sleep(200);
						} catch(Exception e) {}
					} while( ! Button.ENTER.isPressed() );
					
					if( Robot.getDebug() ) {
						RConsole.println("Reseting the tacho counter");
					}

					Motor.B.resetTachoCount();

					break;
				case 1: //Turn debug on
					boolean tmpValue = Robot.getDebug();
					LCD.clearDisplay();
					LCD.refresh();

					if( tmpValue != true ) {
						MainMenuText[1] = "Turn Debug OFF";
						tmpValue = true;
						RConsole.openBluetooth(40000);
					} else {
						MainMenuText[1] = "Turn Debug ON";
						tmpValue = false;
						RConsole.close();
					}	
					Robot.setDebug( tmpValue );
					MainMenu.setItems(MainMenuText);
					break;
				case 2: //Set mode
					LCD.clear();
					String[] ModeMenuText = {"Outline", "Search", "Run All"};
					TextMenu ModeMenu = new TextMenu(ModeMenuText, 0, "Set Mode");

					ModeSelect = ModeMenu.select(ModeSelect);

					if(ModeSelect < 0) return;

					if( Robot.getDebug() ) {
						RConsole.println("Set Mode : " + ModeMenuText[ModeSelect]);
					}
					break;
				case 3: //return to program
					if( Robot.getDebug() ) {
						RConsole.println("" + MainMenuText[MainSelect]);
					}
					switch( ModeSelect ) {
						case 0:
							Robot.RunOutline();
						break;

						case 1:
							Robot.searchDrive(30,30, true);
						break;
		
						case 2:
							Robot.RunOutline();
						break;

						default:
							if( Robot.getDebug() ) {
								RConsole.println("No modeselect has been selected, will quit now.");
							}
						break;
					}
					
					break;
				case 4: //Quit Program
					LCD.clearDisplay();
					LCD.refresh();
					System.out.println("Have a nice day!");
					System.out.println("Goodbye");
					if( Robot.getDebug() ) {//isDebugOn ) {
						RConsole.println("Going to quit...");
						RConsole.println("Have a nice day!");
						RConsole.close();
					}
					System.exit(0);
					break;
			}
		}
	}
}
